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Abstract — The fusion of independently obtained stochastic 
maps by collaborating mobile agents is considered. The proposed 
approach includes two parts: matching of stochastic maps and 
maximum likelihood alignment. In particular, an affine invariant 
hypergraph is constructed for each stochastic map, and a 
bipartite matching via a linear program is used to establish 
landmark correspondence between stochastic maps. A maximum 
likelihood alignment procedure is proposed to determine rotation 
and translation between common landmarks in order to construct 
a global map within a common frame of reference. A main 
feature of the proposed approach is its scalability with respect 
to the number of landmarks: the matching step has polynomial 
complexity and the maximum likelihood alignment is obtained 
in closed form. Experimental validation of the proposed fusion 
approach is performed using the Victoria Park benchmark 
dataset. 

Keywords — data fusion, maximum likelihood estimation, data 
association, mobile robot navigation, hypothesis testing 

I. Introduction 

THE general nature of the problem under consideration 
is to construct a global map of landmarks from the 
individual efforts of collaborating agents that operate outside 
of a global frame of reference. Each agent independently 
builds a vector of estimated landmark locations referred to 
as a stochastic map fl] |2j. Constructing a combined global 
map within a common reference frame from the individual 
maps of the agents is referred to as a problem of fusion of 
stochastic maps. Intuitively, the problem has the interpretation 
of a mathematical jigsaw puzzle: the stochastic maps are the 
disoriented pieces and the sought after global map is the 
completed puzzle. 

A benchmark scenario based on the Victoria Park dataset is 
illustrated in Fig.[T] The satellite image shows the ground truth 
environment from which the stochastic maps are obtained. 
Trees (landmarks) located in the park are mapped from the 
global reference frame of the environment to the individual 
local reference frames of the mobile agents. Each agent 
thus has an independent, but partial model of the explored 
environment. The shared objective of the agents is to build 
a more complete global model of the environment from the 
individually obtained local stochastic maps. 
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Fig. 1 . Stochastic maps of the Victoria Park. The stochastic maps of agent p 
(bottom left) and agent q (bottom right) contain landmark locations estimated 
by each agent. Uncertainties in estimation are indicated by ellipses. The path 
of exploration is shown by dotted lines. Estimation of a common global map 
from the individual stochastic maps, each obtained in a separate coordinate 
system, requires inferring common landmarks in addition to determining a 
common frame of reference. 



Stochastic maps are obtained by independent agents using 
various estimation techniques. In robotics, the solution to 
the simultaneous localization and mapping (SLAM) problem 
provides an agent with a stochastic map of the environment as 
a model of landmark locations (see fl]-[3 and the references 
therein). The focus of this paper, however, is on the fusion - 
rather than building - of stochastic maps. Our starting point 
is at the individual stochastic maps, which are made available 
to a fusion agent for the construction of a global map. 

The fusion problem with multiple agents is challenging for 
several reasons, one being that the problem contains both 
discrete and continuous parts (6). In order to construct a 
global map, the fusion agent must first identify common 
landmarks residing in two separate maps. Using the earlier 
jigsaw analogy, the solver has to first identify common edges 
in order to match the individual pieces. Prior to exchanging 
stochastic maps, the agents are assumed to operate with no 
prior knowledge concerning the common landmarks (i.e., the 
common trees when considering the Victoria Park example) 
that are contained within the individual maps. The problem of 



matching common landmarks is of a combinatorial nature in 
general, which eliminates exhaustive search as an option for 
large maps. 

Even if common landmarks between two maps have been 
identified, the agents are faced with the alignment problem of 
determining not only the best landmark estimates of common 
and uncommon landmarks contained by noisy maps obtained 
in separate coordinate systems, but also to determine the 
spatial parameters of rotation and translation. Describing this 
again in terms of the earlier jigsaw analogy: not only are the 
pieces disoriented, but the edges are also imprecise (which 
makes it harder to see how the pieces fit together). The 
alignment optimization is continuous in nature, but is also 
nonlinear and non-convex in general. 

A. Related work 

The matching and alignment problems considered in this 
paper have been studied in various forms. Thrun and Liu 
[6 1 proposed an SR-tree (Sphere/Rectangle-tree) search in 
consideration of the matching problem. Common landmark 
correspondences and rotation-translation parameters are found 
using an iterative hill climbing approach to match triplet 
combinations formed within a small radius of the landmarks 
in each map. The radius forming the feature vectors of the 
SR-tree, however, would need to be adaptive in order to 
generalize to different environments. Estimates of common 
landmarks are determined separately by a collapsing operation 
performed on matched landmarks in information form (see 
Grime and Durrant-Whyte ®, as well as Sukkarieh et al. 0, 
for further reading on fusion using information filtering). Julier 
and Uhlmann [ 1 1 introduced the covariance intersection 
algorithm as an approach to the data fusion problem. Their 
algorithm uses a convex combination of state information to 
achieve data fusion, but has the limitation that the input data 
must be of the same dimension (which is often not the case of 
stochastic maps built within different regions of exploration). 
Tardos et al. ifTTl . and later Castellanos et al. 1121 . proposed 
map joining as a technique to enable an individual mobile 
robot to construct a global stochastic map based on a sequence 
of local maps. The approach is related to this paper by 
considering the sequence of local maps as being obtained from 
separate robots, but requires knowledge of a base reference to 
construct a global map. 

Williams et al. lf]~3l considered the fusion problem by 
providing parameter estimates of the relative rotation and 
translation between global and local maps. The expressions 
are derived by observing the geometry of the landmarks within 
each map. Our approach is distinct from lfl3ll in that the 
geometry of the landmarks is incorporated in a nonlinear least 
squares solution based on the maximum likelihood principle. 
Several authors such as Zhou and Roumeliotis 03), Andersson 
and Nygards [15], Benedettelli et al. Ifl6l and Aragues et 
al. lfT7l considered rendezvous approaches to the alignment 
problem. Rendezvous approaches, however, are somewhat 
restrictive as the agents are required to be in close proximity. 



The matching approach of this paper is motivated by the 
work of Groth [18| and Ogawa [19) ■ Groth proposed one of 
the earliest matching algorithms in the context of astronomical 
point patterns, where a list of star measurements are matched 
against a known star catalog. In the proposed approach, 
structured point triplets referred to simply as triangles are 
used to match the measurements against the catalog. The 
Groth triangle convention is also incorporated in our approach, 
however the matching approach of Groth is not practical for 
large maps since all possible combinations of triangles are 
considered. An alternative approach was proposed by Ogawa 
|fT9l , which instead incorporated Delaunay triangulations EOl 
to address the star matching problem. This paper therefore uses 
Delaunay triangulations with triangles that follow the Groth 
convention as a graphical model for matching stochastic maps. 
Further insight into the structure of the model is found by 
arranging the triangles in order of increasing perimeter. 

B. Summary of results and organization 

A maximum likelihood framework is proposed for the 
construction of a global map from local stochastic maps. The 
proposed approach includes 1) a landmark matching approach 
referred to as generalized likelihood ratio matching (GLRM) 
and 2) a least squares approach for jointly estimating rotation, 
translation and common landmark locations referred to as 
maximum likelihood alignment (MLA). A Gaussian likelihood 
function is presented as the main proxy for deriving the 
procedures of each step. 

Matching is a step that is performed in the absence of a 
global frame of reference, which requires a technique that 
is affine invariant. To this end, the original stochastic maps 
are represented as directed hypergraphs constructed from 
Delaunay triangulations. The hyperedges of each directed 
hypergraph are constructed from directed Delaunay triangles 
that follow the Groth convention, which leads to an affine 
invariant approach for determining common landmarks. The 
proposed GLRM algorithm uses a generalized likelihood ratio 
as a matching metric in order to obtain globally optimal 
landmark correspondences from the solution of a bipartite 
matching problem. The GLR metric is computed in closed 
form and the bipartite matching is solved in polynomial time 
as a solution to a linear program. 

Once common landmarks are identified, the solution to the 
alignment problem of determining rotation, translation and 
common landmark locations between two stochastic maps 
is computed from the determined common landmarks. The 
main contribution is a closed-form solution to the alignment 
problem as nonlinear non-convex optimization, which makes 
optimal alignment trivial to obtain computationally. 

The remainder of the paper is organized as follows. The 
problem formulation and models used throughout the paper are 
provided in Section[lI] While the alignment and matching steps 
share common likelihood functions, the maximum likelihood 



alignment problem is presented first in Section III in order to 
introduce the proposed solution for closed form computations. 
The problem of determining common landmarks is treated in 



Section IV where we present the GLRM approach. Numerical 
examples and simulations are provided in Section [V] The 
conclusion is given in Section VI and is followed by an 
appendix of proofs. 

II. Model and problem formulation 

A. Ground truth model of landmarks 

A landmark is represented by a vector in M 2 under a specific 
coordinate system. Two collaborating agents p and q each 
estimate the locations of landmarks within a local frame of 
reference. The coordinate systems of p and q are related 
by a rotation with parameter 8 g [— it, it] and a translation 
parameter i e R 2 . Specifically, if n g M 2 is the location of 
a landmark under coordinate system p, the landmark location 
under coordinate system q is then 
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In the general case of m landmark locations /i G in-'", again 
in coordinate system p, the representation in coordinate system 
q is of the form 

fjf = R(9)fj, + Ft 

where R(9) = I m <£> r(9) is the rotation matrix in block 
diagonal form and I m is an m x m identity matrix (the symbol 
(§) is the Kronecker product operator). The matrix F = e m ®l2, 
with e m being an m-vector with all entries equal to 1, applies 
the translation t to each landmark in the map. 

In this paper, without loss of generality, the ground truth of 
the combined map is defined in coordinate system p by the 



vector u = (fi , vi ,v q ) . Common landmarks observed by 
both agents are contained by the vector \i £ M. 2n , where n is 
the number of common landmarks. Landmarks observed by 
only agent p are contained by the vector v p and landmarks 
observed by only agent q are contained by the vector v q . The 
ground truth observed by agent p is then u p = (fi T ,v p ) T , 
which is defined in the coordinate system of agent p, and 
the ground truth of agent q is u q = (/i T ,w^) T in the global 
reference frame, which is observed in coordinate system q as 



III. Maximum likelihood alignment 

This section describes a maximum likelihood approach 
for constructing a global map of landmarks from stochastic 
maps obtained in separate coordinate systems. We begin by 
describing a Gaussian model for the maps and propose a 
closed form solution to the maximum likelihood alignment 
problem of estimating the parameters {/j,, v p , v q , t, 9} under 
the assumption that the common landmarks between the maps 
are known. 



A. Matched Gaussian maps 

Let the random vectors X p and X q represent the noisy 
observations obtained by agents p and q, respectively. Prior to 
fusion, data is collected in the separate coordinate systems of 
the agents (i.e., X p and X q reside in coordinate systems p and 
q, respectively). The statistical model of matched Gaussian 
maps is given as 



X p u p 



X q = R(9)u q + Ft + W q 



(2) 
(3) 



where W p ~ W(0, a 2 1) and W q ~ 7V(0, a q I) are independent 
zero-mean additive Gaussian noise vectors. In order to separate 
the matching and alignment problems, an assumption is made 
that the common landmarks in both maps are known. The 
process of obtaining such a matching, however, is nontrivial 
and combinatorial in general (see Section |IV] for an affine 
invariant procedure for determining common landmarks). 

B. Likelihood decomposition and closed form solution 

Estimators of the parameters {fi, v p , v q , t, 8} are derived by 
considering the likelihood function of the combined global 
map given by 



L(n,v p ,v q ,t,9) = T]exp-^J(fi,v p ,v q ,t, 



(4) 



where ?y is a normalizing constant and the function J, with 
unknown parameters as its arguments, is defined as 



R{8)u q - 
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(1) 



where the subscripts 1 and indicate the partition the map 
into common and uncommon parts. 

In addition to the inherent uncertainty of stochastic maps, 
the challenge of fusion as it relates to constructing a combined 
stochastic maps is that the parameters {t, 9} that relate the 
coordinate systems are unknown and the common landmarks 
observed by both agents are also unknown. The ultimate 
goal of fusion is to estimate the combined map u from the 
stochastic maps of the individual agents when the parameters 
{/j,, 9, t} are unknown. 



J(n,v p ,v q ,t,9) = -^\\x p -u p \\ 2 + ^\\x q - R(9)u q - Ft\\ 2 

&p O q 

(5) 

with x p = (x p T ,Xp T ) T and x q = (x q T ,x q T ) T , corresponding 
to the structure of u p and u q , respectively. By partitioning the 
problem into common and uncommon parts, it immediately 
follows that |5]l decomposes as 



J(fi,v p ,v q ,t,0) = J (v p ,v q ,t,9) + Ji(/i,i, 



(6) 



where J is the squared error function of estimating the 
uncommon landmarks v p and v q , including the transform 
parameters {t, 8}, specified as 



J (v p ,v q ,t,9) 



R {9)v q 



F n t\ 



(7) 



and J\ is the squared error function of estimating the common 
landmarks contained by the vector /i, also including {t, 9}, 
specified as 



JiGM,0) ^ 



1 



rll^-iJx^/i-Fxtl 



(8) 



This decomposition is exploited to minimize the combined 
error function J by minimizing Jo and J\ separately, as stated 
by the following lemma. 

Lemma 1 ( Separable optimization ): Let {fi* , v* , v* , t* , 9* } 
be the global maximum of the likelihood function L, i.e., 



J(fi*,v*v*t*,9*)= min J(fi,v p ,v q ,t,9). (9) 



If the solution {/}, £, 0} is the global minimum of J x given by 

(fi,t,8) — argmin Ji(/i, t, 6), (10) 

then \i* = fi, t* = i, 0* = § and 



v;=x° p) v* q =B%(6)(x° q -F i) 



(11) 



respectively. 



Proof: With the decomposition J = Jo + J\, the proof is 
immediate by noting that 



J Q (x° p ,RT(e)(x°-F o t),t,0) =0 



(12) 



for any {/i, i, 0}. 



Lemma Q] shows that the maximum likelihood solution of 
the combined map specified by u* — (/i* T , v* T , v* T ) T is 
obtained from the nonlinear least squares optimization of 
the non-convex function J\. A global minimum is obtained 
by deriving an equivalent expression of J\ as a sinusoidal 
form parameterized by the unknown rotation parameter 9 (see 
Appendix), which leads to a closed form solution as stated by 
the following theorem. 



Theorem 1 (Closed form MLE): The ML estimators of the 
parameters {/i, t, 9} are given by the following expressions. 
1) The MLE of the rotation parameter 9 is 



sgn(/3) 



yja 2 + 



(13) 



where sgn(-) is the signum function. The coefficients a 
and f3 are given by 



a 



x 1 q r {I n (S>Is)Qxl 



(14) 
(15) 



respectively, where Q = I^n — Fi(F^ Fi)^ 1 with n 
being the number of common landmarks (see Appendix 
for the constant matrices I c and I s ). 

2) The MLE of the translation t is 

t*{9*) = (F[F 1 )- 1 F[ [x\ - R{6*)xl] (16) 

denoted hereafter as t* . 

3) The MLE of the common landmarks fi is 

t f(F) = 4>;xl + 4>* q x 1 q (17) 

denoted hereafter as /j* . The matrix gains 0* and cf>* are 
given by 



'2n 



o'l + o 2 q 



;QR T (9*) 



(18) 
(19) 



respectively. 



Proof: See Appendix. ■ 

Theorem [T] specifies a closed form solution to the ML 
alignment problem using the realizations of matched Gaussian 
maps as data. An important note, however, is that Q = 02x2 
when n = 1, meaning n > 1 common landmarks are required 
to compute the solution of Theorem [T] (a minimum of n = 3 
common landmarks are recommended). 

IV. Generalized Likelihood Ratio Matching 

In a general mapping scenario, the ground truth structure 
observed by the agents is unknown. In particular, if the first 
two entries of X p — x p correspond to the particular landmark, 
then the first two entries of X q = x q correspond to a different 
landmark in general (and likewise with the remaining entries). 
Common landmarks in this case are identified by applying a 
matching procedure to x p and x q with consideration that the 
stochastic maps are obtained in separate coordinate systems 
related by 9 and t. The matching procedure proposed in this 
section is based on the use of landmark triplets referred to as 
triangles, which requires that the maps of each agent contain 
at least three landmarks. 



A. Directed hypergraph model 

Triangles are constructed from the maps of each agent 
by following a direction convention used in the star-pattern 
matching approach of Groth lfl8l . In particular, given three 
landmark locations y a ,Ub,yc S K 2 , the Groth representation 
of a directed triangle is y = (yj ,yf ,yJ) T , which is a vector 
in M 6 with entries that follow the inequality 



\\Va ~ Vb\\ < \\Vb ~ Vc\\ < \\Vc 



Va\ 



(20) 



under the assumption that no two triangle edges have the 
same length. This convention, which is invariant to changes 
in rotation and translation is used to construct the directed 
hypergraphs G p = (V p ,E p ) and G q = (V q ,E q ) from the 
Delaunay triangulations of maps p and q, respectively. The 
landmarks that form the vertices of each graph are contained 
by V p (agent p) and V q (agent q). The resulting directed 
triangles constructed from the maps of p and q are contained 
by the hyperedges E p and E q , respectively. 

B. Hypothesis testing and bipartite matching 

Determining common landmarks from the directed triangles 
of G p and G q is considered as a binary hypothesis testing 
problem. Under hypothesis Ho, the agents observe the ground 
truth directed triangles v pi v q 6 M 6 , which contain a maximum 
of two landmarks in common. Under hypothesis Hi, the 
agents observe a common directed triangle 5 e M. 6 within 
their respective coordinate systems. In this way, Hq is the 
hypothesis of uncommon triangles and Hi is the hypothesis 
of common triangles. The mathematical models of Hq and Hi 
are given by 
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Fig. 2. Monte Carlo performance of hypergraph matching. Receiver operating 
characteristic (ROC) curves, illustrated above, show the performance of the 
detecting triangle matches at various levels of SNR. Each of the curves are 
plots of the probability of detecting a match (Pd) versus the probability of a 
false alarm (Pfa). The dashed line in the lower region of the figure indicates 
the performance of a random guess. 



the uncertain nature of stochastic maps. As illustrated in the 
figure, the performance of the approach degrades gracefully 
with increasing levels of noise (the signal-to-noise ratio, or 
SNR, is discussed in Section [V]). 

Applying the GLRT enables the determination of common 
triangles from data, but not in a one-to-one fashion as required 
to produce a consistent combined map. If triangle i e V , with 
V = {1,2,..., \E p \}, is denoted as y p and triangle j € Q, 
with Q = {1,2, . . . ,\E q \}, is denoted as y q , then triangle 
matches are determined in a one-to-one fashion by formulating 
triangle matching as an assignment problem that seeks to 



Ho 
Hi 



Y„ 



4 1 



8 

R{6)8 + Ft 



(21) 
(22) 



maximize ^ ^ fa (y p ,y q ) z tj 
i=i j=i 

m 

subject to Zij = 1, j = 1, . . 



(24) 
(25) 



respectively. The appropriate matching hypothesis (i.e., Ho or 
Hi) for the directed triangle data Y p = y p and Y q = y q 
is initially unknown. Given the realizations y p and y q from 
the stochastic maps of p and q, respectively, the matching 
hypothesis is determined using a generalized likelihood ratio 
test (GLRT) of the form 



Hy P ,y q ) = 



maxLi(<5, t, 9) 
s,t,e 



Hi 



max Lo{v p ,v q ,t,9) Ho 



(23) 



^ Zlj = l, i = l, 
and e {0, 1}, 



. , m 



(26) 
(27) 



where m = max(|_E p |, \ E q \) and the function fa used in the 
objective function is given by 







otherwise. 



(28) 



where Lk are likelihood functions under Hk, with k € {p, q}, 
and the threshold r is selected to control the level of false 
alarm. The likelihood statistic A(y p ,y q ) is easily computed 
by applying Theorem [T] The performance of the approach in 
the presence of noise, as illustrated by the receiver operating 
characteristic (ROC) curves of Fig. |2j is of interest due to 



The structure of the assignment problem allows for the use of 
standard linear programming routines by relaxing the integer 
constraints to G [0, 1]. The solution of the resulting linear 
program is indicated by the assignment set 

A±{(i€V,j€Q):z*j = l} (29) 
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Fig. 3. Illustration of maximum likelihood fusion using the Victoria Park dataset. (Ground truth) The true landmark locations observed by agent p and agent 
q are indicated by crosses (+) and circles (o), respectively. The agents observe 50 landmarks in common (contained by the vector fj.) with agent p observing 
179 landmarks (contained by u p ) and agent q observing 160 landmarks (contained by u q ). (Stochastic maps) The individual stochastic maps of the agents are 
generated using an additive Gaussian noise model, with the map of agent q being transformed into a separate coordinate system by 9 and t. (GLR matching) 
Common landmarks are determined from a directed hypergraph representation of each stochastic map. Using linear programming and outlier rejection, an 
inlier set of 16 common directed triangles (shaded in gray) are used to estimate the parameters 8 and t. (ML alignment) The closed form MLEs 0* and t* 
(Theorem 1 1 are used to compute the combined map u* = (p* T , v* T , v* T ) T , where the common landmarks contained by fi* are indicated by squares (□). 
Missed detections are easily detected in the common reference frame using nearest neighbor techniques. 



which includes one-to-one assignments of directed triangles 



to be identified as belonging to Hq or H\ . The GLRT ( 23 1 
provides a statistical approach for determining a matching 
hypothesis, but (as indicated in Fig. [2]i the performance of the 
test degrades with increasing noise. A robust detection scheme 
in the presence of uncertainty is to accept the assignments such 
that the MLEs {t*j,9*j} of each triangle assignment form a 
consensus (under rigid body assumptions, the MLEs under Hi 
form a cluster around the true values of t and 9). Maximum 
likelihood estimation of the common parameters {fj,, t, 9} is 
then accomplished by applying Theorem [T] to the landmarks 
of the remaining accepted triangle assignments. 

V. Numerical examples and simulations 

The fusion approach of this paper constructs a global map 
of landmarks in two main steps referred to as GLR matching 
and ML alignment. This section provides an example of 
the ML fusion approach using the Victoria Park dataset and 
evaluates the performance of the matching and alignment steps 
in simulation. An additional requirement of the matching step 
is outlier rejection, which is also discussed in this section. 

A. Illustration of maximum likelihood fusion 

Consider a Delaunay triangulation constructed from the 
ground truth landmarks observed by agent p and agent q with 
edge lengths {1$ : i = 1, 2, . . . , to}. By modeling the variance 
of the stochastic maps as a 2 = a 2 q = a 1 , the signal-to-noise 
ratio (SNR) in decibels follows as 



zero-mean Gaussian noise is additive to the landmarks rather 
than to the edge lengths directly. The discussion of SNR in 



SNR dB = 10 log - 



(30) 



the remainder of the paper is in reference to ( 30 1. 

An illustration of the proposed ML fusion approach is 
shown in Fig. [3] The ground truth landmarks shown in the 
figure are obtained by applying the the sparse local submap 
joining filter (SLSJF) proposed by Huang et al. fT\\ to the 
Victoria Park dataset. The ground truth is partitioned into two 
vectors u p and u q as models of the ground truth landmark 
locations observed by agent p and agent q, respectively (see 
Section [H}. The stochastic maps of each agent are generated 
using the additive noise model discussed in Section III at an 



with a signal variance of a 2 = — J^iLi ^1 computed from the 
ground truth points and a noise variance of <7^ = 2a 2 since the 



SNR of 30dB. The rotation (in radians) and translation (in 
meters) applied to the stochastic map agent q are 9 = 0.7854 
and t — (100, 5) T , respectively. As illustrated in the figure, 9 
and t parameterize a spatial transform of the stochastic map 
of agent q in reference to the ground truth coordinate frame 
(i.e., the coordinate system of agent p). 

The directed hypergraph representation of each stochas- 
tic map is used by the GLR matching to determine the 
common directed triangles across the coordinate systems of 
the agents (using the Groth convention enables to the de- 
termination of common landmarks from common directed 
triangles). The number of directed triangles in hypergraphs 
p and q are \E P \ = 340 and \E q \ = 304, respectively. Due 
to the uncertainty of the stochastic maps, outlier rejection 
(discussed shortly) is required to determine an inlier set of 
matching triangles. Theorem [T] is then applied to the inlier 
landmarks to compute the closed form MLEs 9* = 0.7878 
and t* = (100.0860, 4.9120) T . Once the maps of each agent 
are represented within a common frame of reference, missed 
detections in the matching are easily found using common 
data association techniques such as nearest neighbor and 
maximum likelihood. Maximum likelihood estimation of the 
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directed triangle index (agent p) 

(b) 
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(d) 



Fig. 4. GLR matching statistics and triangle assignments (Victoria Park). 

(a) The matrix of likelihood statistics is of the form C = [A(yt,y^)], with 
the directed triangles of p and q indexed as i £ "P and j £ Q, respectively 
(the band diagonal structure of C is due to arranging the triangles in order of 
increasing perimeter). Darker entries indicate a higher likelihood of a directed 
triangle match. Markers are used to indicated entry C'ij corresponding to 

(b) the true directed triangle matches, (c) the triangle matches specified by 
the linear program and (d) the remaining 16 inlier matches that result from 
applying outlier rejection. 



combined map u* = (p, 



, v* ) immediately follows 



from Lemma Q] and Theorem |l] 

Inliers and outliers of the assignment set A are identified 
as follows. Consider the matrix of likelihood statistics shown 
in Fig. |4j which is used by the linear program to construct 
the assignment set (the true directed triangle matches, as well 
as the matches contained by A, are indicated by markers). As 
illustrated in the figure, the linear programming formulation 
computes 304 triangle assignments, however there are only 
66 common triangles in truth. Each entry of A is associated 
with closed form MLEs of the parameters 9 and t of the form 
6*j and t*j, respectively, as illustrated in Fig. [5] (only the ML 
estimators of t are shown to simplify the illustration). A simple 
heuristic for determining the inlier set is to recursively reject 
the individual MLEs with the largest sample MSE relative to 
the sample mean. The rejection is repeated until the sample 
variance of the remaining estimators falls below a specified 
threshold, as indicated by the 16 MLEs accepted as inliers 
in Fig. [5] This heuristic is preferred over the more standard 
RANSAC algorithm |22| due to the conservative nature of the 
heuristic in choosing the inlier set (the inlier set is known 
a priori to form a cluster under rigid body assumptions) in 
addition to the guarantee that the number of iterations of 
the heuristic is no greater than the cardinality of A. The 
corresponding accepted matches are shown at the bottom right 
of Fig. |4] which subsequently leads to the GLR matching 
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Fig. 5. Application of closed form MLE to outlier rejection (Victoria Park). 
Each entry of the matrix [A(j/|,, J/J)] of likelihood statistics is associated with 
ML estimators of 8 and t of the form 8*- and t*., respectively, with i £ V 
and j £ Q. The estimators of the parameter t are shown in the 2D plot 
above (a similar plot is created in 3D by incorporating the ML estimators of 
the parameter 8). The true value of t is indicated by the intersection of the 
dashed lines. Inlier matches (black circles) are indicated by the MLEs that 
form a cluster around the true value of t, with outliers (gray crosses) indicated 
by the entries that fall outside the cluster. The inlier set is indicated by 16 
closed form MLEs that cluster around the true values of 8 and t. 



diagram of Fig. [3] 

B. Performance with simulated ground truth 

The performance of GLR matching is considered, followed 
by the performance of ML alignment. A simple simulation 
of ground truth is shown in Fig. [6] The ground truth vector 
u contains 30 landmark locations drawn as samples from 
the uniform distribution. The stochastic maps of p and q 
are generated from the ground truth with complete overlap. 
The GLR matching approach uses Delaunay triangulations to 
compute a generalized likelihood ratio as a metric for matching 
directed triangles. Fig. [2] shows the decline in performance 
of the likelihood statistic to determine common triangles with 
decreasing SNR. The performance of the underlying Delaunay 
triangulations is considered in Fig. [6] The simulation shows 
that a gradual decline in the percentage of common triangles 
can be expected with decreasing SNR even in a scenario where 
the stochastic maps have a complete overlap in ground truth. 

Perhaps a more subtle consideration in evaluating the GLR 
matching is the distribution of the ground truth landmarks. 
Two simple examples are shown in Fig. [7] In the first example, 
the ground truth is generated as samples from a uniform 
distribution. In this case, the GLR statistics computed from 
stochastic maps of the ground truth exhibits a banded structure 
similar to the Victoria Park example. In such an environment, 
the GLR matching approach is capable of determining com- 
mon directed triangles between the stochastic maps. In the 
second example, however, the ground truth is generated as a 
deterministic grid. In this case, the banded structure of the 
likelihood statistics is lost due to a significantly large number 



of equally likelihood triangles. The performance of the GLR 
matching in this case is severely impacted not necessarily 
due to sensor and process noise, but rather to the type of 
environment explored by the mobile agents. For this reason, 
the deterministic grid serves as a counter example of the GLR 
matching approach. 

The performance of the ML alignment approach is shown 
in Fig. [8] The Monte Carlo simulation of the figure is based 
on stochastic maps generated from the ground truth landmarks 
shown in Fig. [6] Given the common landmarks in each map, 
the MSE performance of the estimators exhibit are linear trend 
in performance degradation with decreasing SNR. A similar 
trend is observed in the case of unknown landmarks when the 
variance of the noise is relatively low. At lower SNR, however, 
a greater decrease in MSE performance is observed relative 
to the known landmark case due not only to a decreased 
performance of the GLR matching statistic (Fig. [2]), but also 
to a decrease in common triangles due to noise (Fig. [6]). An 
additional decrease in performance is due to missed detections 
in the GLR matching (as seen in the Victoria Park example 
at the right of Fig. |3j. As SNR increases, however, the plots 
exhibit a convergence in MSE performance. 
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Fig. 6. Ground truth simulation and common directed triangles, (a) 
Landmarks contained by the ground truth vector u with x- and y- coordinates 
being samples drawn from a uniform distribution, (b) The stochastic maps of 
p and q are generated from the ground truth with complete overlap, however 
the percentage of common triangles declines with increasing noise (as shown 
in the Monte Carlo simulation above). 



VI. Conclusion 

This paper considered the problem of constructing a global 
map of landmarks from the stochastic maps of collaborating 
agents - fusion of stochastic maps. The problem can be 
formulated as a mixed integer-parameter estimation problem 
from which landmarks common to each agent is aligned 
under a global coordinate system. Under this framework, the 
optimal fusion of stochastic maps can be accomplished using 
the maximum likelihood principle. Unfortunately, however, 
the complexity of the true ML solution is prohibitive, which 
leads to a partitioning of the problem into two steps: (i) 
matching landmarks via bipartite hypergraph matching using 
generalized likelihood ratio as a quality measure and (ii) 
maximum likelihood alignment to obtain the estimated of the 
combined map under a common coordinate frame. 

The main advantage of the proposed approach, in spite of 
its suboptimality due to the separate treatment of the matching 
(which includes outlier rejection) and alignment problems, is 
the comprehensive nature of the procedure: a global map is 
found in spite of the individual stochastic maps being obtained 
in separate coordinate systems without prior knowledge of 
common landmarks. In simulations, the performance of the 
proposed approach is reasonable at high SNR but deteriorates 
with increasing noise, which is largely due the matching step. 
One way to improve the performance may be to impose 
neighborhood constraints on triangles in addition to better 
heuristics for removing outliers. Further computation gains 
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Fig. 7. Ground truth environments and hypergraph matching statistics, (a) 
A ground truth environment, shown a the left, is generated using 256 points 
drawn from a uniform distribution. At the right are GLR matching statistics of 
stochastic maps generated from the ground truth at 30dB . (b) Another ground 
truth environment of 256 points is generated as a deterministic grid. Unlike 
the uniform case, the matching statistics in this case indicate a large number 
of equally likely directed triangle matches. 
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respectively, with nonzero entries corresponding to the cosine 
and sine functions of the rotation matrix r(9). In addition, the 
following lemma is used in the proof of Theorem [T] 

Lemma 2: The matrix Q is an idempotent and symmetric 
matrix that commutes with a block diagonal matrix of the form 
A = I n © B, with B e R 2x2 . 

Proof of Lemma |2j The idempotence and symmetry 
properties are immediate from the structure of the matrix Q. 
The product of the matrix FF T and the block diagonal matrix 
A = I n <g) B is given by 

FF T A = \{e n el)I n ] ® [I 2 B] 

= [I n {e n el)]®[Bh] 

= [I n ®B][{e n el)®h] 

= AFF T . 







Since FF T A = AFF T and F T F = ij 2 , it follows that 
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SNR (dB) 




(b) 

Fig. 8. Monte Carlo performance of closed form MLE. The MSE (in dB) 
of estimating (a) the combined map u and (b) the transform parameters t 
and 9 are shown in the figures above. The performance of the ML estimators 
under the model of matched Gaussian maps, which the common landmarks are 
known, is shown by the dashed lines. In the general case that the common 
landmarks are unknown, the estimators are computed from an inlier set of 
matched directed triangles as determined by the GLR matching. 



may also be found by exploiting the banded structure of the 
likelihood matching statistics. 
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Appendix 

The constant 2x2 matrices I c and I s used in Theorem [T] 
are defined as 



= A— -FF T A 

n 

= A- -AFF T 

n 

= A[I 2n -F(F T F)- 1 F T ] 
= AQ 

which proves that Q and A commute. ■ 

Proof of Theorem [7} Minimizing (|8]l with respect to 
(w.r.t.) /i leads to 



fl(0,t) 



^-2^.2 
a y a q 



\x p + \R T {6){x q -Ft) 



and minimizing ([8]l w.r.t. t leads to 

i(e,n) = (F T F)- 1 F T {x q -R{e)n) 



(31) 



(32) 



Using the evaluation /i = fi(6,t) in (32i results in the MLE 
of t as a function of 8 given by 



t*{8) = (F 1 Fy^F 1 (xg-R(e)x p ). 



(33) 



Applying the evaluation t = t*(6) in (31 1 leads to the MLE 
of /j, as a function of 8 given by 



H*{0) = (f>p(8)x p + cj> q (8)x q . 



(34) 



Using the symmetry and idempotence properties of the matrix 
Q (Lemma [2]), it follows from the expression (34i that 



1 
1 



and L 



Hip -Ai* (*)l I s 



Kp | \ Xq R{0')Xp \ \qtq 
Kp\\%q ~ -^(^)^pIIq 



where n p = I ^jqr^g j ■ Similarly, it follows from (33 1 and 



341 that 



\x q -R(9) f i*(9)-Ft*(9)\\ 2 = K q \\x g -R(6)x p \\ 2 QTQ 

= K q \\ Xq - R(e)x p \\ 2 Q 



{ (J 2 \ ^ 

where n q — ( 2 2 2 ) ■ Using these simplifications to define 



Jt(6) 4 &JiQf{0),r(6),6) 
= l\\x q -R(6)x p \\ 2 Q 



(35) 



where k = jrKp+^K, and expanding the norm in the right 
hand side (RHS) of 63} as 



||z 9 -i?(0):Zj,||| = xTR T (8)QR(9)x p + xTQx q 

-2x T q QR{9)x p , (36) 

it follows from Lemma [2] that the first term on the RHS of 
d36li reduces to 



xTR T (0)QR(0)x p = xlR T {0)R{9)Qx r , 



(37) 



so that from (|35j, (|36| and (|37j>, (0) reduces to 

Jlifi) = \{x T p Qx p + x T q Qx q - 2x T q R{Q)Qx p ). (38) 
Notice in the last term on the RHS of d38b that 



(39) 



x T q R{6)Qx p = x T q [R c {9)+R s {9)]Qx p 

= x q R c (9)Qx p + x q R s (9)Qx p 



where R c {9) = (I n ®I c ) cos(0) and i? s (6») = (I„®I S ) sin(0), 
meaning that 

-2 X TR(9)Qx p = 2acos((9) + 2/?sin(0) 

where a = —x q (I n ® I c )Qx p and /? = —x q (I n ® I s )Qx p , 
from which it immediately follows that 



(6>) = a cos(6») + /3 sin(0) + 7 



(40) 



where 7=5 (x p Qx p + x^Qx q ). By virtue of the sinusoidal 
form (40 1, it follows that ./^(f?) has a unique minimum for 



/1 given by ( 16 1 and ( 17 1, respectively. 



G [— 7T, 7r] given by ( 13 1, which leads to the MLEs of £ and 
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